# Demo program for FIR filter module 6th Feb 2015
# Outputs a swept frequency sine wave on Dac1
# Timer interrupt reads the analog input, filters it, and outputs the result on Dac 2.
# Requires a link between X5 and X7

import math
import pyb
import array
from fir import fir
import micropython
micropython.alloc_emergency_exception_buf(100)

# Define hardware
dac1 = pyb.DAC(1)
dac2 = pyb.DAC(2)
adc  = pyb.ADC("X7")
tim = pyb.Timer(4, freq=2000) # Sampling freq 10KHz is about the limit 14KHz without constraint

# Data for FIR filter: narrowband filter designed to isolate a 100Hz signal (@2Ksps)
coeffs = array.array('i', (-22340,-3286,-2750,-1678,-135,1757,3821,5842,7586,8829,9375,9089,7912,5881,3127
  ,-125,-3575,-6875,-9661,-11591,-12391,-11880,-10004,-6854,-2663,2207,7292,12068,16007,18627
  ,19553,18560,15614,10880,4727,-2301,-9531,-16224,-21648,-25157,-26261,-24696,-20455,-13815,-5318
  ,4267,14010,22912,30002,34442,35623,33239,27338,18335,6989,-5657,-18372,-29853,-38858,-44338
  ,-45554,-42171,-34313,-22568,-7959,8167,24205,38538,49613,56146,57273,52646,42488,27594,9283
  ,-10722,-30452,-47888,-61165,-68769,-69705,-63621,-50867,-32488,-10138,14059,37713,58406,73939,82558
  ,83149,75378,59759,37601,10940,-17678,-45420,-69457,-87244,-96794,-96895,-87255,-68561,-42437,-11303
  ,21841,53710,81059,101009,111355,110803,99126,77217,47023,11377,-26271,-62183,-92711,-114661,-125634
  ,-124280,-110461,-85294,-51059,-11000,30986,70726,104192,127903,139298,136995,120977,92591,54461,10233
  ,-35774,-78987,-115037,-140201,-151790,-148433,-130234,-98790,-57047,-9030,40550,86764,124951,151199,162738
  ,158236,137946,103700,58750,7463,-45096,-93698,-133543,-160456,-171710,-166013,-143785,-107096,-59479,-5585
  ,49271,99638,140473,167651,178380,171491,147566,108888,59225,3455,-52909,-104270,-145507,-172488,-182485
  ,-174450,-149132,-108998,-57996,-1167,55864,107439,148439,174791,183876,174791,148439,107439,55864,-1167
  ,-57996,-108998,-149132,-174450,-182485,-172488,-145507,-104270,-52909,3455,59225,108888,147566,171491,178380
  ,167651,140473,99638,49271,-5585,-59479,-107096,-143785,-166013,-171710,-160456,-133543,-93698,-45096,7463
  ,58750,103700,137946,158236,162738,151199,124951,86764,40550,-9030,-57047,-98790,-130234,-148433,-151790
  ,-140201,-115037,-78987,-35774,10233,54461,92591,120977,136995,139298,127903,104192,70726,30986,-11000
  ,-51059,-85294,-110461,-124280,-125634,-114661,-92711,-62183,-26271,11377,47023,77217,99126,110803,111355
  ,101009,81059,53710,21841,-11303,-42437,-68561,-87255,-96895,-96794,-87244,-69457,-45420,-17678,10940
  ,37601,59759,75378,83149,82558,73939,58406,37713,14059,-10138,-32488,-50867,-63621,-69705,-68769
  ,-61165,-47888,-30452,-10722,9283,27594,42488,52646,57273,56146,49613,38538,24205,8167,-7959
  ,-22568,-34313,-42171,-45554,-44338,-38858,-29853,-18372,-5657,6989,18335,27338,33239,35623,34442
  ,30002,22912,14010,4267,-5318,-13815,-20455,-24696,-26261,-25157,-21648,-16224,-9531,-2301,4727
  ,10880,15614,18560,19553,18627,16007,12068,7292,2207,-2663,-6854,-10004,-11880,-12391,-11591
  ,-9661,-6875,-3575,-125,3127,5881,7912,9089,9375,8829,7586,5842,3821,1757,-135
  ,-1678,-2750,-3286,-22340))

# Data for FIR filter Pass (@2Ksps) 0-40Hz Stop 80Hz->
lpfcoeffs = array.array('i', (72, 47, 61, 75, 90, 105, 119, 132, 142, 149, 152, 149,
  140, 125, 102, 71, 33, -12, -65, -123, -187, -254, -322, -389, -453, -511, -561,
  -599, -622, -628, -615, -579, -519, -435, -324, -187, -23, 165, 375, 607, 855,
  1118, 1389, 1666, 1941, 2212, 2472, 2715, 2938, 3135, 3303, 3437, 3535, 3594,
  3614, 3594, 3535, 3437, 3303, 3135, 2938, 2715, 2472, 2212, 1941, 1666, 1389,
  1118, 855, 607, 375, 165, -23, -187, -324, -435, -519, -579, -615, -628, -622,
  -599, -561, -511, -453, -389, -322, -254, -187, -123, -65, -12, 33, 71, 102, 125,
  140,  149, 152, 149, 142, 132, 119, 105, 90, 75, 61, 47, 72))
ncoeffs = len(coeffs)
data = array.array('i', [0]*(ncoeffs +3)) # Scratchpad must be three larger than coeffs
data[0] = ncoeffs
data[1] = 20

# Data input, filter and output
def cb(timer):
    dac2.write(fir(data, coeffs, adc.read()) // 1000000)

def cb1(timer):         # For filters with 0 DC response
    val = fir(data, coeffs, adc.read()) // 256 # Filter amd scale
    dac2.write(max(0, min(255, val+128))) # Constrain, shift (no DC from bandpass) and output


tim.callback(cb1)

# Sweep generator
def sine_sweep(start, end, mult):     # Emit sinewave on DAC1
    buf = bytearray(100)
    for i in range(len(buf)):
        buf[i] = 128 + int(110 * math.sin(2 * math.pi * i / len(buf)))

    freq = start
    while True:
        dac1.write_timed(buf, int(freq) * len(buf), mode=pyb.DAC.CIRCULAR)
        print(freq, "Hz")
        pyb.delay(2500)
        freq *= mult
        if freq > end:
            freq = start

sine_sweep(70, 200, 1.02)
#sine_sweep(10, 400, 1.33)

